import numpy as np

class VectorPocessor:
    def __init__(self) -> None:
        self.standard_vector=None

    def set_standard(self,vector):
        self.standard_vector=vector
        self.std_center = np.array(self.standard_vector["center"])
        std_x = np.array(self.standard_vector["bottom_edge"]["unit_vector"])  # 标准x轴
        std_y = np.array(self.standard_vector["left_edge"]["unit_vector"])    # 标准y轴
        std_z = np.cross(std_x, std_y)                   # 标准z轴
        # print(np.dot(self.std_x, self.std_y))
        std_z = std_z / np.linalg.norm(std_z)            # 单位化
        self.R_std = np.vstack([std_x, std_y, std_z]).T  # 标准坐标系到世界坐标系的旋转
        self.std_x = np.dot(self.R_std.T, std_x)
        self.std_y = np.dot(self.R_std.T, std_y)
        self.std_z = std_z
        self.vector={
            "center": self.std_center.tolist(),
            "bottom_vec": self.std_x.tolist(),
            "left_vec": self.std_y.tolist(),
            "z_vec": self.std_z.tolist(),
            "bottom_angle": 0,
            "left_angle": 0
        }
    def get_xyzr_by_vector(self, edges_vector):
        """
        将边缘点转换到标准坐标系并计算角度关系
        
        参数:
            edges: 原始边缘点数据
            standard_vector: 标准坐标系定义
            
        返回:
            包含以下信息的字典:
            - center: 转换后的中心点坐标
            - bottom_vec: 转换后的x方向向量
            - left_vec: 转换后的y方向向量
            - z_vec: 转换后的z方向向量
            - bottom_angle: bottom向量与x轴的夹角(度, 逆时针为正)
            - left_angle: left向量与y轴的夹角(度, 逆时针为正)
        """
        if not edges_vector:
            print("错误: 输入的edges_vector为空")
            return None
        
        # 获取初始中心点
        edges_center = np.array(edges_vector["center"])
        
        # 确定x和y方向向量
        if "bottom_edge" in edges_vector:
            # 正常情况：使用bottom_edge作为x方向
            edges_x = np.array(edges_vector["bottom_edge"]["unit_vector"])
            edges_y = np.array(edges_vector["left_edge"]["unit_vector"])
            # 不需要移动中心点
        elif "top_edge" in edges_vector:
            # 当没有bottom_edge时：使用top_edge作为x方向，left_edge反向作为y方向
            edges_x = np.array(edges_vector["top_edge"]["unit_vector"])
            edges_y = -np.array(edges_vector["left_edge"]["unit_vector"])  # 反向
            # 沿left_edge方向移动2.5
            if "left_edge" in edges_vector:
                left_vec = np.array(edges_vector["left_edge"]["unit_vector"])
                edges_center = edges_center + left_vec * 2.5
        else:
            print("错误: 既没有bottom_edge也没有top_edge")
            return None

        # 转换中心点
        transformed_center = np.dot(self.R_std.T, edges_center - self.std_center)
        
        # 转换到标准坐标系
        transformed_x = np.dot(self.R_std.T, edges_x)
        transformed_y = np.dot(self.R_std.T, edges_y)
        transformed_z = np.cross(transformed_x, transformed_y)
        transformed_z = transformed_z / np.linalg.norm(transformed_z)  # 单位化
        
        def calculate_angle(v1, v2):
            """计算v1到v2的夹角(逆时针方向为正)"""
            # 只考虑xy平面的分量
            v1_2d = v1[:2] / np.linalg.norm(v1[:2])
            v2_2d = v2[:2] / np.linalg.norm(v2[:2])
            
            dot = np.dot(v1_2d, v2_2d)
            det = v1_2d[0] * v2_2d[1] - v1_2d[1] * v2_2d[0]  # 2D叉积
            angle = np.arctan2(det, dot)
            return np.degrees(angle)
        
        if transformed_x[0]<0:
            transformed_x[0]=-transformed_x[0]

        if transformed_y[1]<0:
            transformed_y[1]=-transformed_y[1]
        # 计算角度
        bottom_angle = calculate_angle(self.std_x, transformed_x)
        left_angle = calculate_angle(self.std_y, transformed_y)
        
        return {
            "center": transformed_center.tolist(),
            "bottom_vec": transformed_x.tolist(),
            "left_vec": transformed_y.tolist(),
            "z_vec": transformed_z.tolist(),
            "bottom_angle": bottom_angle,
            "left_angle": left_angle
        }




######## 以下为弃用代码 ###############

    def edges_to_vector(self, edges1):
        """
        计算左边缘和底边缘之间的角度关系
        
        参数:
            edges1: 包含拟合边缘点的字典，应有:
                   - left_edge: 左边缘的点集
                   - bottom_edge: 底边缘的点集
                   
        返回:
            包含以下信息的字典:
            - angle_diff: 两向量夹角与90度的差值(绝对值，单位:度)
            - center: 两条边最近点的中心点(3D坐标)
            - left_vec: 左边缘的单位方向向量
            - bottom_vec: 底边缘的单位方向向量
            - is_perpendicular: 是否近似垂直(angle_diff < 10度)
        """
        # 获取两条边的端点
        left_edge = edges1.get("left_edge", [])
        bottom_edge = edges1.get("bottom_edge", [])
        
        if len(left_edge) < 2 or len(bottom_edge) < 2:
            print("警告: 边缘点不足，无法计算向量关系")
            return None
        
        # 提取每条边的两个端点（分别为第一个非None点和最后一个非None点）
        def get_first_and_last_valid(points):
            first = next((np.array(pt) for pt in points if pt is not None), None)
            last = next((np.array(pt) for pt in reversed(points) if pt is not None), None)
            return first, last
        left_p1, left_p2 = get_first_and_last_valid(left_edge)
        bottom_p1, bottom_p2 = get_first_and_last_valid(bottom_edge)
        
        # 计算所有可能的点对距离
        points = {
            'left1': left_p1,
            'left2': left_p2,
            'bottom1': bottom_p1,
            'bottom2': bottom_p2
        }
        
        # 找出不同边上距离最近的两个点
        min_dist = float('inf')
        closest_pair = None
        
        for left_key in ['left1', 'left2']:
            for bottom_key in ['bottom1', 'bottom2']:
                dist = np.linalg.norm(points[left_key] - points[bottom_key])
                if dist < min_dist:
                    min_dist = dist
                    closest_pair = (points[left_key], points[bottom_key])
        
        # print(closest_pair)
        # 计算中心点
        center = (closest_pair[0] + closest_pair[1]) / 2
        
        # 确定每条边的起点(距离中心点近的点)
        def get_ordered_points(p1, p2, center):
            d1 = np.linalg.norm(p1 - center)
            d2 = np.linalg.norm(p2 - center)
            return (p1, p2) if d1 < d2 else (p2, p1)
        
        # 获取有序点并计算方向向量
        left_start, left_end = get_ordered_points(left_p1, left_p2, center)
        left_vec = left_end - left_start
        left_vec = left_vec / np.linalg.norm(left_vec)
        
        bottom_start, bottom_end = get_ordered_points(bottom_p1, bottom_p2, center)
        bottom_vec = bottom_end - bottom_start
        bottom_vec = bottom_vec / np.linalg.norm(bottom_vec)
        
        # 计算两向量夹角(弧度)
        dot_product = np.clip(np.dot(left_vec, bottom_vec), -1.0, 1.0)
        angle_rad = np.arccos(dot_product)
        angle_deg = np.degrees(angle_rad)
        
        # 计算与90度的差值
        angle_diff = abs(angle_deg - 90)
        # print("edges1:",edges1)
        return {
            "angle_diff": angle_diff,
            "center": edges1["center"],
            "left_vec": left_vec.tolist(),
            "bottom_vec": bottom_vec.tolist(),
            "is_perpendicular": angle_diff < 10  # 差值小于10度视为垂直
        }